/*
 * @author Ted Meyer
 */

package edu.wpi.first.wpilibj.robot;

import edu.wpi.first.wpilibj.*;



public class Bot3464 extends SimpleRobot {
    
    Thread drive;
    Thread bridge;
    Thread elevation;
    Thread ballPickUp;
    Thread gunThread;
    
    Jaguar driveJag1;
    Jaguar driveJag2;
    Jaguar bridgeLiftJag;
    Jaguar shooterElevation;
    Jaguar belt;
    Jaguar gun;
    Jaguar trigger;
    
    Joystick js2 = new Joystick(1);   //secondary Joystick (override controls)
    Joystick js = new Joystick(2);    //Main Joystick (driving et cetera)
    
    Joystick[] joys;
    
    Jaguar[] driveJags;
    Jaguar[] gunJags;
    
    
    Thread autoangle;
    Thread autoshooting;
    
    
    public void autonomous() {
        DriverStationLCD dlcd = DriverStationLCD.getInstance();
        
        PrintMotor pm = new PrintMotor(dlcd);
        Motor beltMotor = new BeltMotor(js2, belt);
        Motor driveMotor = new DriveMotor(driveJags, js, js2, pm);
        GunMotor gunMotor = new GunMotor(js2, gunJags, (DriveMotor)driveMotor, (BeltMotor)beltMotor, pm);
        
        ballPickUp = new Thread(beltMotor);
        gunThread = new Thread(gunMotor);
        
        ballPickUp.start();
        gunThread.start();
        
        gunMotor.setSpeed(.2);
        gunMotor.autoFire = true;
        
        try{Thread.sleep(5000);}catch(Exception e){}
        
        ((BeltMotor)beltMotor).auto = true;
        
        try{Thread.sleep(3700);}catch(Exception e){}
        
        ((BeltMotor)beltMotor).auto = false;
        
        try{Thread.sleep(3000);}catch(Exception e){}
        
        ((BeltMotor)beltMotor).auto = true;
        
    }

    
    public void operatorControl() {
        DriverStationLCD dlcd = DriverStationLCD.getInstance();
        
        PrintMotor pm = new PrintMotor(dlcd);
        Motor driveMotor = new DriveMotor(driveJags, js, js2, pm);
        Motor bridgeMotor = new BridgeMotor(js, bridgeLiftJag);
        Motor beltMotor = new BeltMotor(js2, belt);
        Motor angleMotor = new AngleMotor(shooterElevation,js,js2,pm);
        Motor gunMotor = new GunMotor(js2, gunJags, (DriveMotor)driveMotor, (BeltMotor)beltMotor, pm);
        pm.reset();
        
    	drive = new Thread(driveMotor);
        bridge = new Thread(bridgeMotor);
        ballPickUp = new Thread(beltMotor);
        gunThread = new Thread(gunMotor);
        elevation = new Thread(angleMotor);
        Thread printing = new Thread(pm);
        
        
        drive.start();
        bridge.start();
        ballPickUp.start();
        gunThread.start();
        elevation.start();
        printing.start();
    }
    
    public void robotInit(){
        
        driveJag1 = new Jaguar(1);
        driveJag2 = new Jaguar(2);
        bridgeLiftJag = new Jaguar(3);
        shooterElevation = new Jaguar(4);
        belt = new Jaguar(6);
        gun = new Jaguar(7);
        trigger = new Jaguar(8);
        
        driveJags = new Jaguar[2];
        driveJags[0] = driveJag1;
        driveJags[1] = driveJag2;
        
        gunJags = new Jaguar[2];
        gunJags[0] = trigger;
        gunJags[1] = gun;
        
        joys = new Joystick[2];
        joys[0] = js;
        joys[1] = js2;
        
        
        System.out.println("\n\n");
        System.out.println("--------------------------------");
        System.out.println("drive1:           "+driveJag1);
        System.out.println("drive2:           "+driveJag2);
        System.out.println("BridgeLift:       "+bridgeLiftJag);
        System.out.println("AngleElevation:   "+shooterElevation);
        System.out.println("belt:             "+belt);
        System.out.println("gun:              "+gun);
        System.out.println("\n\n");
        System.out.println("joystick1:     "+js);
        System.out.println("joystick2:     "+js2);
        System.out.println("--------------------------------");
        System.out.println("\n\n");
    }
}
